
use cmsis_rtos2_rs::{osWaitForever, static_mem::{mq_size, EventFlags, MessageQueue}};
use periph_utils::{can::CAN, GpioConfig, RCC};
use periph_utils::stm32f1::{self, stm32f103::{interrupt, Interrupt, NVIC}};
use utils::ByteBuffer;

use crate::app::proto::Proto;



const FLAG_TX: u32 = 0x01;
const FLAG_PKG: u32 = 0x02;

const MSG_COUNT: usize = 32;

struct Context {
    flags: EventFlags,
    mq: MessageQueue<Frame, MSG_COUNT, { mq_size::<Frame>(MSG_COUNT) }>,
    write_buf: ByteBuffer<255>,
}

impl Context {

    const fn uninit() -> Self {
        Self {
            flags: EventFlags::uninit(),
            mq: MessageQueue::uninit(),
            write_buf: ByteBuffer::const_new()
        }
    }

    unsafe fn init_once(&self) {
        self.flags.init_once("cmd_can");
        self.mq.init_once("cmd_can");
        self.flags.set_flags(FLAG_PKG | FLAG_TX);
    }
}

static CTX: Context = Context::uninit();

#[derive(GpioConfig)]
#[gpio(
    init = "gpio_init",
    io(name = "tx", io = "PA12", mode = "AF_PP"),
    io(name = "rx", io = "PA11", mode = "IN_FLOAT"),
    io(name = "s", io = "PA15", mode = "IN_FLOAT")
)]
struct Pin;

#[derive(Clone, Copy)]
pub struct Frame {
    pub id: u32,
    len: u8,
    buf: [u8; 8],
}

impl Frame {
    pub fn data<'a>(&'a self) -> &'a [u8] {
        &self.buf[..(self.len as usize)]
    }

    pub fn pkg_seq(&self) -> u8 {
        self.id as u8
    }

    pub fn dst(&self) -> u8 {
        (self.id >> 8) as u8
    }

    pub fn src(&self) -> u8 {
        (self.id >> 16) as u8
    }

    pub fn is_complete(&self) -> bool {
        (self.id & (0x01 << 24)) != 0x00
    }

    pub fn read() -> Self {
        let id = CAN.rx[0].rir.read().bits() >> 3;
        let len = CAN.rx[0].rdtr.read().dlc().bits();
        let tmp0 = CAN.rx[0].rdlr.read().bits();
        let tmp1 = CAN.rx[0].rdhr.read().bits();
        CAN.rfr[0].write(|w| w.rfom().release());

        Self {
            id,
            len,
            buf: [
                tmp0 as u8,
                (tmp0 >> 8) as u8,
                (tmp0 >> 16) as u8,
                (tmp0 >> 24) as u8,
                tmp1 as u8,
                (tmp1 >> 8) as u8,
                (tmp1 >> 16) as u8,
                (tmp1 >> 24) as u8,
            ],
        }
    }
}

fn filter_config() {
    CAN.fmr.write(|w| w.finit().set_bit());
    CAN.fa1r
        .write(|w| w.fact0().clear_bit()); // 过滤器禁用

    CAN.fs1r.write(|w| w.fsc0().set_bit()); // 单个32位

    CAN.fm1r.write(|w| w.fbm0().clear_bit()); // 标识符屏蔽模式
    CAN.ffa1r.write(|w| w.ffa0().clear_bit()); // 关联到FIFO 0

    const MASK: u32 = (0xFF00 << 3) | 0x04;
    
    #[cfg(not(feature = "cp_drop"))]
    {
        use super::super::config::address::MY;
        const VALUE: u32 = (((MY as u32) << 8) << 3) | 0x04;
        CAN.fb[0].fr1.write(|w| w.fb().variant(VALUE));
        CAN.fb[0].fr2.write(|w| w.fb().variant(MASK));
    }

    #[cfg(feature = "cp_drop")]
    {
        use super::super::config::address::IPC;
        const VALUE: u32 = (((IPC as u32) << 8) << 3) | 0x04;  
        CAN.fb[0].fr1.write(|w| w.fb().variant(VALUE));
        CAN.fb[0].fr2.write(|w| w.fb().variant(MASK));
    }

    CAN.fa1r.write(|w| w.fact0().set_bit()); // 过滤器启用
    CAN.fmr.write(|w| w.finit().clear_bit());
}

pub unsafe fn init() {
    CTX.init_once();
    
    Pin::gpio_init();

    RCC.apb1enr.modify(|_, w| w.canen().enabled());
    CAN.mcr
        .write(|w| w.inrq().set_bit().abom().set_bit().nart().set_bit());

    while CAN.msr.read().inak().bit_is_clear() {}

    const TBS2: u8 = 2;
    const TBS1: u8 = 1;
    const BRP: u16 = 5;

    CAN.btr
        .write(|w| w.ts2().variant(TBS2).ts1().variant(TBS1).brp().variant(BRP));

    CAN.mcr.modify(|_, w| w.inrq().clear_bit());
    while CAN.msr.read().inak().bit_is_set() {}

    filter_config();

    CAN.ier.write(|w| w.tmeie().enabled().fmpie0().enabled());

    NVIC::unmask(Interrupt::USB_HP_CAN_TX);
    NVIC::unmask(Interrupt::USB_LP_CAN_RX0);
}

fn write_can_frame(id: u32, ptr: *const u8, len: u8) {
    _ = CTX.flags.wait_any_flags(FLAG_TX, osWaitForever);

    CAN.tx[0].tdtr.write(|w| w.dlc().variant(len));

    let tmp0 = unsafe {
        (ptr.read() as u32)
            | ((ptr.add(1).read() as u32) << 8)
            | ((ptr.add(2).read() as u32) << 16)
            | ((ptr.add(3).read() as u32) << 24)
    };
    let tmp1 = unsafe {
        (ptr.add(4).read() as u32)
            | ((ptr.add(5).read() as u32) << 8)
            | ((ptr.add(6).read() as u32) << 16)
            | ((ptr.add(7).read() as u32) << 24)
    };

    CAN.tx[0].tdlr.write(|w| unsafe { w.bits(tmp0) });
    CAN.tx[0].tdhr.write(|w| unsafe { w.bits(tmp1) });
    CAN.tx[0]
        .tir
        .write(|w| unsafe { w.bits((id << 3) | 0x04 | 0x01) });
}

fn write_pkg(dst: u8, src: u8, buf: &[u8]) {
 
    let mut index = 0;
    let mut count = 0;
    
    while index < buf.len() {
        let mut n = buf.len() - index;
        if n > 8 {
            n = 8;
        }
        let flag = (index + n) >= buf.len();

        let id = count | ((dst as u32) << 8) | ((src as u32) << 16) | ((flag as u32) << 24);

        write_can_frame(id, buf[index..].as_ptr(), n as u8);
        index += n;
        count += 1;
    }
}

pub fn write<'a, T: Proto<'a> + ?Sized>(x: &T) {
    _ = CTX.flags.wait_any_flags(FLAG_PKG, osWaitForever);
    
    unsafe {

        let all_buf = CTX.write_buf.as_mut_slice();
        let write_buf = x.serialize(all_buf);
        let src = write_buf[5];
        let dst = write_buf[6];
        write_pkg(dst, src, write_buf);
    }
 
    _ = CTX.flags.set_flags(FLAG_PKG);
}


pub fn take() -> Frame {
    CTX.mq.take()
}

#[interrupt]
unsafe fn USB_HP_CAN_TX() {
    if CAN.tsr.read().rqcp0().bit_is_set() {
        CAN.tsr.write(|w| w.rqcp0().set_bit());
        CTX.flags.set_flags(FLAG_TX);
    }
}

#[interrupt]
unsafe fn USB_LP_CAN_RX0() {
    if CAN.rfr[0].read().fmp().bits() != 0 {
        let frame = Frame::read();
        _ = CTX.mq.post(&frame);
    }
}
